#include <zenilib.h>
#include "GameObject.h"
#include <iostream>

using namespace std;
using namespace Zeni;


GameObject::GameObject(const Vector2 &pos_,
					   const Vector2 &scale_,
					   const float &rot_,
					   const Vector2 &vel_,
					   const float &rotvel_,
					   const Vector2 &accel_,
					   const float &rotaccel_)
: pos(pos_),
lastpos(pos_),
centerofmass(pos_),
scale(scale_),
rot(rot_),
lastrot(rot_),
vel(vel_),
rotvel(rotvel_),
accel(accel_),
rotaccel(rotaccel_),
mass(1.0f),
inertia(0.0f),
elasticity(0.0f),
flipped(false)
{
	poly = Poly(pos - scale / 2, pos + scale / 2);
}

GameObject::GameObject(GameObject &rhs) 
: scale(rhs.scale),
pos(rhs.pos),
lastpos(rhs.lastpos),
centerofmass(rhs.pos),
rot(rhs.rot),
lastrot(rhs.lastrot),
vel(rhs.vel),
rotvel(rhs.rotvel),
accel(rhs.accel),
rotaccel(rhs.rotaccel),
mass(rhs.mass),
inertia(rhs.inertia),
elasticity(rhs.elasticity),
flipped(rhs.flipped)
{
}

GameObject & GameObject::operator=(GameObject &rhs) {
	scale = rhs.scale;
	pos = rhs.pos;
	lastpos = rhs.lastpos;
	centerofmass = rhs.centerofmass;
	rot = rhs.rot;
	lastrot = rhs.lastrot;
	vel = rhs.vel;
	rotvel = rhs.rotvel;
	accel = rhs.accel;
	rotaccel = rhs.rotaccel;
	mass = rhs.mass;
	inertia = rhs.inertia;
	elasticity = rhs.elasticity;
	flipped = rhs.flipped;
	return *this;
}

GameObject::~GameObject() {}

void GameObject::update(const float &timestep) {
	vel += timestep * accel;
	if (vel.y > 1000){
		vel.y = 1000;
	}
	rotvel += timestep * rotaccel;
	if (!vel.zero()){
		setPos(pos + timestep * vel);
	}
	if (rotvel != 0){
		setRot(rot + timestep * rotvel);
	}
}

void GameObject::setScale(Vector2 & newscale)
{
	Vector2 delta = newscale / scale;
	scale = newscale;
	for (int i = 0; i < poly.size; i++){
		poly[i] = pos + multiply((poly[i] - pos), delta);
	}
	centerofmass = pos + multiply((centerofmass - pos), delta);
}

void GameObject::setPos(Vector2 & newpos)
{
	Vector2 delta = newpos - pos;
	lastpos = pos;
	pos = newpos;
	for (int i = 0; i < poly.size; i++){
		poly[i] += delta;
	}
	centerofmass += delta;
}

void GameObject::setRot(float newrot)
{
	float delta = rot - newrot; //backward on purpose for calculation
	lastrot = rot;
	rot = newrot;
	for (int i = 0; i < poly.size; i++){
		poly[i] = poly[i].rotateAround(pos, delta);
	}
	centerofmass = centerofmass.rotateAround(pos, delta);
}

float GameObject::distanceFrom(GameObject *obj)
{
	return (obj->getPos() - this->getPos()).length();
}

bool GameObject::sameRegion(GameObject *obj)
{
	return (distanceFrom(obj) < obj->getRadius() + this->getRadius());
}

void GameObject::render()
{
	for (int i = 0; i < textures.size(); i++){
		if (textures[i] != ""){
			render_image(textures[i],
						 (pos - scale / 2).toPoint2f(),
						 (pos + scale / 2).toPoint2f(),
						 rot,
						 1,
						 pos.toPoint2f(),
						 flipped,
						 colorFilters[i]);
		}
	}
}

Collision::Collision()
{
	m_collisionReported = false;
	m_object[0] = m_object[1] = NULL;
	m_ncoll = Vector2(0, 0);
	m_mtd = Vector2(0, 0);
	m_tcoll = 0.0f;
}

Collision::Collision(GameObject* a, GameObject* b, float dt)
{
	if (!a->sameRegion(b)){
		m_collisionReported = false;
		return;
	}

	m_collisionReported = false;
	m_object[0]	= a;
	m_object[1]	= b;
	m_contacts	= ContactInfo();
	m_ncoll	= Vector2(0, 0);
	m_mtd = Vector2(0, 0);
	m_tcoll	= 0.0f;

	// polygons in world space at the time of collision	
	m_poly[0] = a->getPoly();
	m_poly[1] = b->getPoly();
	
	// find collision
	Vector2 delta = (a->getVel() - b->getVel()) * dt;
	m_collisionInfo = m_poly[0].collide(m_poly[1], delta);
	m_collisionReported = (m_collisionInfo.m_overlapped || m_collisionInfo.m_collided);
		
	if(!m_collisionReported)
		return;

	// convert collision info into collison plane info
	if(m_collisionInfo.m_overlapped)
	{
		if(m_collisionInfo.m_mtdLengthSquared <= 0.00001f)
		{
			m_collisionReported = false;
			return;
		}

		m_ncoll = m_collisionInfo.m_mtd / sqrt(m_collisionInfo.m_mtdLengthSquared);
		m_tcoll = 0.0f;
		m_mtd   = m_collisionInfo.m_mtd;
	}
	else if(m_collisionInfo.m_collided)
	{
		m_ncoll = m_collisionInfo.m_Nenter;
		m_tcoll = m_collisionInfo.m_tenter;
	}
	m_tcoll *= dt;
	m_ncoll.normalize();

	// find contact poly at time of collision
	m_poly[0].translate(a->getVel() * m_tcoll);
	m_poly[1].translate(b->getVel() * m_tcoll);
	
	SupportPoints asup = m_poly[0].getSupports(m_ncoll);
	SupportPoints bsup = m_poly[1].getSupports(-m_ncoll);

	m_contacts = ContactInfo(asup, bsup);
};

void Collision::seperateBodies()
{
	GameObject* a = m_object[0];
	GameObject* b = m_object[1];
	// overlapped. then separate the bodies.
	a->setPos(a->getPos() + m_mtd * (a->getInverseMass() / (a->getInverseMass() + b->getInverseMass())));
	b->setPos(b->getPos() - m_mtd * (b->getInverseMass() / (a->getInverseMass() + b->getInverseMass())));
}

void Collision::unseperateBodies()
{
	GameObject* a = m_object[0];
	GameObject* b = m_object[1];
	// overlapped. then separate the bodies.
	a->setPos(a->getPos() - m_mtd * (a->getInverseMass() / (a->getInverseMass() + b->getInverseMass())));
	b->setPos(b->getPos() + m_mtd * (b->getInverseMass() / (a->getInverseMass() + b->getInverseMass())));
}

void Collision::applyResponse(float dt, bool rigid)
{
	if(!m_collisionReported)
		return;

	seperateBodies();

	GameObject* a = m_object[0];
	GameObject* b = m_object[1];

	// move to time of collision
	a->setPos(a->getPos() + a->getVel() * m_tcoll);
	b->setPos(b->getPos() + b->getVel() * m_tcoll);

	//determine average vector of contacts poly to center
	Vector2 ra = Vector2(0, 0);
	Vector2 rb = Vector2(0, 0);
	for(int i = 0; i < m_contacts.m_count; i++){
		ra += m_contacts.m_contact[i].m_position[0] - a->getPos();
		rb += m_contacts.m_contact[i].m_position[1] - b->getPos();
	}
	ra /= m_contacts.m_count;
	rb /= m_contacts.m_count;

	Vector2 va = a->getVel() + ra.perp() * a->getRotVel();
	Vector2 vb = b->getVel() + rb.perp() * b->getRotVel();
	Vector2 v  = (va - vb);
	Vector2 nc = m_ncoll; // collision normal
	//// contact poly separating, no impulses.
	if(v * nc > 0.0f) return;

	Vector2 vt = v - (v * m_ncoll) * m_ncoll;
	Vector2 nf = -vt; 
	nf.normalize(); // friction normal

	// collision impulse
	float jc = (v * nc) / ((a->getInverseMass() + b->getInverseMass()) + 
						   (ra ^ nc) * (ra ^ nc) * a->getInverseInertia() + 
						   (rb ^ nc) * (rb ^ nc) * b->getInverseInertia());

	// friction impulse
	float jf = 0;
	if (rigid){
		jf = (v * nf) / ((a->getInverseMass() + b->getInverseMass()) + 
						 (ra ^ nf) * (ra ^ nf) * a->getInverseInertia() + 
						 (rb ^ nf) * (rb ^ nf) * b->getInverseInertia());
	}
	// total impulse restituted
	Vector2 impulseA = nc * (jc * -(1.0f + a->getElasticity())) + nf * (jf * -1.0f);
	Vector2 impulseB = nc * (jc * -(1.0f + b->getElasticity())) + nf * (jf * -1.0f);

	a->setVel(a->getVel() + impulseA * a->getInverseMass());
	b->setVel(b->getVel() - impulseB * b->getInverseMass());
	if (rigid && dt != 0.0){
		a->setRotVel(a->getRotVel() - (ra ^ impulseA) * a->getInverseInertia() / dt);
		b->setRotVel(b->getRotVel() + (rb ^ impulseB) * b->getInverseInertia() / dt);
	}
}